import redis
import time
import math
import json
import statistics
import time
import geopy.distance
import pickle
from scipy.stats import expon
import threading
import msgpack
from scipy.spatial.transform import Rotation as R
import subprocess


class AutoDrive(object):
    def __init__(self):
        self.r = redis.Redis(db=0)
        self.new_job = True
        self.task_index = int(r.get('job_index'))
        self.last_oy = 0
        self.base_speed = 6000
        self.d_queue = []
        self.timestamp = time.time()

        self.distance_to_end_point = 10000  # m
        self.last_gps_timestamp = None
        self.operation_status = None

        self.abs_min_b = 100000
        self.msg_map = None
        self.msg_map_set = False

        self.msg_map_waypoints = []

    def restart_sensor(self, post_detect_on=False, camera_1_on=False, new_job=False, radar_side=None, stop=True):
        if stop:
            self.r.set('forward_left_speed', 0)
            self.r.set('forward_right_speed', 0)

        self.r.set('camera_1_command', 'off')
        self.r.set('post_detect_command', 'off')

        self.r.set('gps_command', 'on')

        self.r.set('radar_1', 'off')    # left forward
        self.r.set('radar_2', 'off')    # left rear
        self.r.set('radar_3', 'off')    # right forward
        self.r.set('radar_4', 'off')    # right rear

        time.sleep(1)

        if camera_1_on:
            self.r.set('camera_1_command', 'on')

        if post_detect_on:
            self.r.set('post_detect_command', 'on')

        if radar_side == 'left':
            self.r.set('radar_1', 'on')
            self.r.set('radar_2', 'on')
        elif radar_side == 'right':
            self.r.set('radar_3', 'on')
            self.r.set('radar_4', 'on')
        else:
            pass

        if new_job:
            # self.task_index = self.task_index + 1
            # self.r.set('job_index', self.task_index)
            self.new_job = True

            self.r.lpop('job_queue')
            self.r.set('slam_termination', '0')
        else:
            self.new_job = False

        self.last_oy = 0
        self.d_queue = []
        self.distance_to_end_point = 10000

        self.r.set('tx', 0)
        self.r.set('ty', 0)
        self.r.set('tz', 0)

        self.r.set('oy', 0)

        self.r.set('slam_termination', '1')

        self.timestamp = time.time()

        self.abs_min_b = 100000
        self.msg_map_set = False
        self.msg_map = None
        self.msg_map_waypoints = []

    def auto_straight_walk(self, constant_distance, side):
        d_window = 14

        px = 15
        dx = 6

        if len(self.d_queue) > d_window:
            p = self.d_queue[0] - constant_distance
            d = self.d_queue[0] - self.d_queue[d_window]
        else:
            p = self.d_queue[0] - constant_distance
            d = 0

        deviation = p * px + d * dx

        if side == 'right':
            deviation = -deviation

        self.auto_straight(deviation, 1300)

        if len(self.d_queue) > d_window*2:
            self.d_queue = self.d_queue[:d_window+1]

    def auto_straight(self, deviation, max_level):
        if deviation > max_level:
            deviation = max_level

        if deviation < -max_level:
            deviation = -max_level

        self.r.set('forward_left_speed', int(self.base_speed - deviation))
        self.r.set('forward_right_speed', int(self.base_speed + deviation))

    def cumulative_distance(self):
        tx = float(self.r.get('tx'))
        ty = float(self.r.get('ty'))
        tz = float(self.r.get('tz'))

        d = int(math.sqrt(tx ** 2 + ty ** 2 + tz ** 2))

        self.r.set('cumulative_distance', d)

        return d

    def auto_straight_forward_radar(self, constant_distance, side):
        time.sleep(0.1)
        if self.new_job:
            self.restart_sensor(post_detect_on=False, camera_1_on=True, radar_side=side)

        distance = self.cumulative_distance()
        # print(distance)

        if distance >= 13500:
            self.restart_sensor(new_job=True)
        else:
            if len(self.d_queue) > 10:
                print(statistics.mean(self.d_queue))

            # if len(self.d_queue) > 15 and statistics.mean(self.d_queue) < 200:
            #     self.base_speed = 10000
            # else:
            #     self.base_speed = 4000

            if distance <= 3000:
                self.base_speed = 4000
            else:
                self.base_speed = 10000

            x1 = int(r.get('radar_1_distance'))
            x2 = int(r.get('radar_2_distance'))

            a = 1070

            d = x1 * a / (math.sqrt((x2-x1)**2 + a**2))

            print(d)

            if (d > constant_distance - 1000) and (d < constant_distance + 1000):
                self.d_queue = [d] + self.d_queue
                self.auto_straight_walk(constant_distance, side)
            else:
                self.auto_straight(0, self.base_speed)

    def gps_straight_forward(self, start_point, end_point, off_set=0):
        if self.new_job:
            self.restart_sensor(post_detect_on=False, camera_1_on=False, stop=True)
            # self.r.set('pole_status', 'DOWN')
            # self.operation_status = 'DOWN'
        try:
            gps_timestamp = float(self.r.get('gps_timestamp'))
            if (time.time() - gps_timestamp < 1.1) and (self.last_gps_timestamp != gps_timestamp):
                self.last_gps_timestamp = gps_timestamp
                print('valid gps')

                lat = float(self.r.get('lat'))
                lon = float(self.r.get('lon'))
                hAcc = float(self.r.get('hAcc'))

                if hAcc > 100:
                    self.r.set('forward_left_speed', 0)
                    self.r.set('forward_right_speed', 0)
                else:
                    d = (lat - start_point[0])*(end_point[1] - start_point[1]) - (lon - start_point[1])*(end_point[0] - start_point[0])

                    a = geopy.distance.distance((start_point[0], start_point[1]), (lat, lon)).m
                    b = geopy.distance.distance((end_point[0], end_point[1]), (lat, lon)).m
                    c = geopy.distance.distance((start_point[0], start_point[1]), (end_point[0], end_point[1])).m

                    print(b)

                    cos = (b ** 2 + c ** 2 - a ** 2) / (2 * b * c)

                    if cos < 0:
                        self.restart_sensor(new_job=True)

                    p = (a + b + c)/2

                    h = math.sqrt(p*(p-a)*(p-b)*(p-c)) * 2 / c

                    if d > 0:
                        h = -h*1000
                        print('left')
                    else:
                        h = h*1000
                        print('right')

                    print(h)

                    self.d_queue = [h] + self.d_queue

                    if a < 3:
                        self.base_speed = 3000
                    else:
                        self.base_speed = int(expon.pdf(abs(h)/400) * 3000)

                    print(self.base_speed)

                    # PID
                    px = 6
                    dx = 32

                    if len(self.d_queue) > 1:
                        p = h + off_set
                        d = self.d_queue[0] - self.d_queue[1]
                    else:
                        p = 0
                        d = 0

                    deviation = (p * px + d * dx)*1.5

                    self.auto_straight(deviation, 2700)

                    if len(self.d_queue) > 10:
                        self.d_queue = self.d_queue[:10]

            else:
                pass
                # print('not valid')
        except:
            pass

    def fix_speed_turn(self, speed_left, speed_right, time_lock):
        if self.new_job:
            self.restart_sensor(post_detect_on=False, camera_1_on=False, radar_side='no', stop=True)

        if time.time() - self.timestamp > time_lock:
            self.restart_sensor(new_job=True, stop=False)
        else:
            self.r.set('forward_left_speed', speed_left)
            self.r.set('forward_right_speed', speed_right)

        time.sleep(0.3)

    def turn_around(self, side, turn_d, speed_left, time_lock):
        if self.new_job:
            self.restart_sensor(post_detect_on=False, camera_1_on=True, radar_side='no')

        oy = float(self.r.get('oy'))

        speed_right = int(speed_left*(1 + 1500/(turn_d/2)))

        print(self.last_oy)
        print(oy)

        if side == 'right':
            speed_left, speed_right = speed_right, speed_left

        print(time.time() - self.timestamp)

        if (abs(self.last_oy - oy) > 1.5) or (abs(oy) > 0.99) or ((time.time() - self.timestamp) > time_lock):
            self.restart_sensor(new_job=True)
        else:
            # self.r.set('forward_left_speed', speed_left)
            # self.r.set('forward_right_speed', speed_right)

            self.r.set('forward_left_speed', speed_left)
            self.r.set('forward_right_speed', speed_right)

            self.last_oy = oy

        time.sleep(0.3)

    def openvslam_line_following(self, msg_map):
        def distance_between(point_A, point_B):
            return math.sqrt((point_A[0] - point_B[0]) ** 2 + (point_A[2] - point_B[2]) ** 2)

        if r.get('slam_termination').decode("utf-8") == '1':
            r.set('slam_termination', '0')
            p = subprocess.Popen('../../openvslam-master/build/run_camera_localization  -v ../../openvslam-master/build/orb_vocab/orb_vocab.dbow2 -n 4 -c ../../openvslam-master/build/asmaa/config.yaml -p ../../openvslam-master/build/%s' % msg_map, shell=True)
            time.sleep(10)

        if not self.msg_map_set:
            with open(msg_map, 'rb') as f:
                data = msgpack.unpack(f)

            self.msg_map_waypoints = [
                [list(-(R.from_quat(data['keyframes'][k]['rot_cw'])).as_matrix().transpose().dot(
                    data['keyframes'][k]['trans_cw'])),
                 data['keyframes'][k]['src_frm_id']]
                for k in data['keyframes']
            ]

            self.msg_map_waypoints.sort(key=lambda e: e[1])

        current_location_index = 0

        cam_center_x = float(r.get('cam_center_x').decode("utf-8"))
        cam_center_y = float(r.get('cam_center_y').decode("utf-8"))
        cam_center_z = float(r.get('cam_center_z').decode("utf-8"))

        current_pose = [cam_center_x, cam_center_y, cam_center_z]
        # print(current_pose)

        min_distance = 1000

        i = 0
        while i < len(self.msg_map_waypoints) - 1:
            min_distance_t = distance_between(self.msg_map_waypoints[i][0], current_pose)
            if min_distance_t < min_distance:
                min_distance = min_distance_t
                current_location_index = i
            i = i + 1
        print(current_location_index)
        print(len(self.msg_map_waypoints))
        if current_location_index + 4 >= len(self.msg_map_waypoints):
            self.restart_sensor(new_job=True)
        else:
            start_point = self.msg_map_waypoints[current_location_index][0]
            end_point = self.msg_map_waypoints[current_location_index+4][0]

            a = distance_between(start_point, current_pose)
            b = distance_between(end_point, current_pose)
            c = distance_between(start_point, end_point)

            p = (a + b + c) / 2

            h = math.sqrt(p * (p - a) * (p - b) * (p - c)) * 2 / c
            # print(h)

            d = (current_pose[0] - start_point[0]) * (end_point[1] - start_point[1]) - (
                        current_pose[2] - start_point[2]) * (end_point[0] - start_point[0])

            if d < 0:
                h = +h*5000
                print('right')
            else:
                h = -h*5000
                print('left')

            print(h)
            self.base_speed = int(expon.pdf(abs(h) / 400) * 7000)

            self.d_queue = [h] + self.d_queue

            px = 35
            dx = 20

            if len(self.d_queue) > 4:
                p = h
                d = self.d_queue[0] - self.d_queue[4]
            else:
                p = 0
                d = 0

            deviation = (p * px + d * dx) * 1

            self.auto_straight(deviation, 2700)

            if len(self.d_queue) > 10:
                self.d_queue = self.d_queue[:10]

            print(self.r.get('forward_left_speed'))
            print(self.r.get('forward_right_speed'))

    def auto_task(self):
        with open('gps_location.pickle', 'rb') as f:
            gps_location = pickle.load(f)

        right_turn_step_1 = (self.fix_speed_turn, [9000, 4000, 36])
        right_turn_step_2 = (self.fix_speed_turn, [4000, -4000, 20])

        left_turn_step_1 = (self.fix_speed_turn, [4000, 9000, 33])
        left_turn_step_2 = (self.fix_speed_turn, [-4000, 4000, 22])

        def line(startpoint, endpoint, offset=0):
            self.gps_straight_forward(gps_location[startpoint], gps_location[endpoint], offset)

        def right_u_turn():
            self.fix_speed_turn(10000, 3000, 25)

        def left_u_turn():
            self.fix_speed_turn(3000, 10000, 25)

        def left():
            self.fix_speed_turn(3000, 10000, 13)

        def right():
            self.fix_speed_turn(10000, 3000, 13)

        def left_adjust():
            self.fix_speed_turn(-6000, 6000, 2)

        def right_adjust():
            self.fix_speed_turn(6000, -6000, 2)

        def operation_wait(code, time_to_wait):
            self.restart_sensor(new_job=True, stop=True)
            self.r.set('pole_status', code)
            time.sleep(int(time_to_wait))

        def wait_operation(code, time_to_wait):
            self.restart_sensor(post_detect_on=False, camera_1_on=False, radar_side='no', stop=True)
            time.sleep(int(time_to_wait))
            self.r.set('pole_status', code)

        def operation_thread(code, time_to_wait):
            self.r.lpop('job_queue')
            x = threading.Thread(target=wait_operation, args=(code, time_to_wait))
            x.start()

        def openvslam_line(msg_map):
            self.openvslam_line_following(msg_map)

        left_turn = (self.fix_speed_turn, [5000, 10000, 28])
        right_turn = (self.fix_speed_turn, [10000, 5000, 28])

        # right - ; left +

        function_dict = {
            'line': line,
            'right_u_turn': right_u_turn,
            'left_u_turn': left_u_turn,
            'left': left,
            'right': right,
            'left_adjust': left_adjust,
            'right_adjust': right_adjust,
            'operation': operation_wait,
            'operation_thread': operation_thread,
            'openvslam_line': openvslam_line,
        }

        if self.r.llen('job_queue') > 0:
            job = json.loads(r.lindex('job_queue', 0))
            function_dict[job[0]](*job[1])
        else:
            self.restart_sensor(post_detect_on=False, camera_1_on=False)


if __name__ == '__main__':
    r = redis.Redis(db=0)

    autoDrive = AutoDrive()
    autoDrive.restart_sensor(new_job=True)

    while True:
        try:
            autoDrive.auto_task()
            time.sleep(0.1)
        except KeyboardInterrupt:
            autoDrive.restart_sensor(post_detect_on=False, camera_1_on=False)
            break
